﻿using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using Framework.Math.KalmanDemo;

namespace BridgeShips {
    public class NoiseFilterPointBuf {

        private LinkedList<Vector3> m_ListPoints = null;
        private float m_AutoReleaseInterval;
        private float m_ExpireTime;
        private int m_Capatity = 10;
        private float m_Threshold = 100.0f; //7 m/s
        /*
         *    (a) -d1-(b)-d2-(c) 
         *    (a)-d3-(c) 
         *    (d1 || d2)>d3 && d1>dis || d2>dis 
         *    remove b  point;
         */
        private float[] m_DistanceCompare;  //    

        float[] idealNoNoise_X = new float[6];
        float[] idealNoNoise_Z  = new float[6];
        float[] idealNoNoiseVelocity = new float[6];
        public NoiseFilterPointBuf( int capatity, float expiretime, float autoReleaseInterval ) {
            m_ListPoints = new LinkedList<Vector3>();
            m_DistanceCompare = new float[3];
            m_Capatity = capatity;
            m_ExpireTime = expiretime;
            m_AutoReleaseInterval = autoReleaseInterval;

        }

        public LinkedList<Vector3> ListPoints {
            get {
                return m_ListPoints;
            }
        }

        public int Count {
            get {
                return m_ListPoints.Count;
            }
        }
        public void Clear() {
            if (true) {
                m_ListPoints.Clear();
            }
        }

        /// <summary>
        /// 获取或设置对象池的容量
        /// </summary>
        public int Capacity {
            get {
                return m_Capatity;
            }
            set {
                m_Capatity = value;
            }
        }

        public void Collect( object obj ) {
            if (obj == null) {
                return;
            }
            LinkedListNode<Vector3> current = m_ListPoints.First;
            while (current != null) {

                return;
            }
            current = current.Next;
        }

        /*
         *   pre->current->last
         * 
         */
        public bool AddLast( Vector3 value ) {
            if (Count > 0 && Vector3.Equals( m_ListPoints.Last.Value, value )) {
                return false;
            }
            m_ListPoints.AddFirst( value );
            //CreateObj(value);
            if (m_ListPoints.Count == m_Capatity)
            {
                RemoveDriftPoints();
            }
            return true;
        }

        public void CreateObj(Vector3 Pos)
        {
            GameObject obj = GameObject.Instantiate(Resources.Load("Prefabs\\Prefabs\\Ships\\DangBan") as GameObject);
            //GameObject obj =  GameObject.CreatePrimitive(PrimitiveType.Capsule);
            obj.transform.position = Pos;
        }

        public void RemoveDriftPoints() {
            ArrayList deleteList = new ArrayList();
            LinkedListNode<Vector3> current = m_ListPoints.First;
            int index = 0;
            Vector3 PrePoint = current.Value;
            while (current != null) {
                if (index != 0) {
                    Vector3 CurPoint = current.Value;
                    float disValue = Vector3.Distance( PrePoint, CurPoint );
                    m_DistanceCompare[0] = disValue;
                    bool beAdd = false;
                    if (disValue > m_Threshold) {
                        deleteList.Add( CurPoint );
                        beAdd = true;
                    }
                    else {

                    }
                  
                    if (current.Next != null) {
                        m_DistanceCompare[2] = Vector3.Distance( PrePoint, current.Next.Value );
                        m_DistanceCompare[1] = Vector3.Distance( CurPoint, current.Next.Value );
                        if (CompareDistance() && !beAdd) {
                            deleteList.Add( CurPoint );
                        }
                    }
                    PrePoint = current.Value;
                }
                index++;
                current = current.Next;
            }
            //remove drift point
            foreach(var dp in deleteList)
            {
                m_ListPoints.Remove((Vector3)dp);
            }
            deleteList.Clear();
        }

        public bool CompareDistance() {
            if ((m_DistanceCompare[0] > m_DistanceCompare[2] || m_DistanceCompare[1] > m_DistanceCompare[2])
                && m_DistanceCompare[2] < 100.0f) {
                return true;
            }

            return false;
        }
        public void RemoveFirst() {
            if (m_ListPoints.Count > 0)
                m_ListPoints.RemoveFirst();
        }

        public float[] FilterNoiseValue( float[] idealNoNoise, float[] idealNoNoiseVelocity ) {

            // Add in wind, auto-pilot on our rocket always successfully adapts so
            // that the effect by the wind is guassian process noise.
            float[] withProcessNoise = new float[Count];
            float[] withProcessNoiseVel = new float[Count];
            for (int i = 0; i < Count; i++) {
                // Add in 1 foot = 1 std. deviation of process noise.
                withProcessNoise[i] = idealNoNoise[i];
                withProcessNoiseVel[i] = idealNoNoiseVelocity[i];
            }

            // Add in measurement noise, the GPS on the rocket measures less often
            // than our data but we can just skip things.
            // Assume a very good GPS, accurate to +/- 6 feet even at these speeds.
            // 2 foot = 1 std dev. is about +/- 6 feet
            float[] withAllNoise = new float[Count];
            float[] withAllNoiseVel = new float[Count];
            for (int i = 0; i < Count; i++) {
                // Add in 1 foot = 1 std. deviation of process noise.
                withAllNoise[i] = withProcessNoise[i];
                withAllNoiseVel[i] = withProcessNoiseVel[i];
            }

            Kalman2D k = new Kalman2D();
            k.Reset( 0.01f, 0.1f, 0.1f, 400, 0 );

            // Assume we get to see every other measurement we calculated, and use
            // the others as the points to compare for estimates.
            // Run the filter, note our time unit is 1.
            float[] kalman = new float[idealNoNoise.Length];
            float[] vel = new float[idealNoNoise.Length];
            float[] kGain = new float[idealNoNoise.Length];
            for (int i = 0; i < Count; i++) {
                if (i == 0) {
                    kalman[0] = 0;
                }
                else {
                    kalman[i] = k.Update( withAllNoise[i], withAllNoiseVel[i], 1 );
                }
            }
            return kalman;
        }

        public void IterateList() {
            LinkedListNode<Vector3> current = m_ListPoints.First;
            int index = 0;
            while (current != null) {
                idealNoNoise_X[index] = current.Value.x;
                idealNoNoise_Z[index] = current.Value.z;
                idealNoNoiseVelocity[index] = 0.1f;
                index++;
                current = current.Next;
            }
            float[] kalmanX = FilterNoiseValue( idealNoNoise_X, idealNoNoiseVelocity );
            float[] kalmanZ = FilterNoiseValue( idealNoNoise_Z, idealNoNoiseVelocity );
            CombineValueToList( kalmanX, kalmanZ );

            //提交到lineChart 图表
            PresectToLineChart();
        }

        public void PresectToLineChart() {

        }

        public void CombineValueToList( float[] kalmanX, float[] kalmanZ ) {
            LinkedListNode<Vector3> current = m_ListPoints.First;
            int index = 0;
            while (current != null) {
                current.Value.Set( kalmanX[index], 7.0f, kalmanZ[index] );
                index++;
                current = current.Next;
            }
        }

        // Update is called once per frame
        public void UpdateBuf(  ) {
            if (Count == m_Capatity) {
                IterateList();
            }
        }
    }
}